#include <Messenger.h>

/*
 
 
 */
 
#include <Servo.h> 

Servo steeringServo;

const int steeringServoPin = 5;


const int motorAin1Pin = 13;
const int motorAin2Pin = 12;
const int motorApwmPin = 11;

const int motorBin1Pin = 8;
const int motorBin2Pin = 7;
const int motorBpwmPin = 6;




// Instantiate Messenger object with the default separator (the space character)
Messenger message = Messenger(); 


  


void setMotorA(int motorASpeed) 
{
  if(motorASpeed!=0){
    if(motorASpeed>0) {
      digitalWrite(motorAin1Pin,HIGH);
      digitalWrite(motorAin2Pin,LOW);
    } else {
      digitalWrite(motorAin1Pin,LOW);
      digitalWrite(motorAin2Pin,HIGH);
    }
    analogWrite(motorApwmPin,abs(motorASpeed));
  } else {
    digitalWrite(motorAin1Pin,LOW);
    digitalWrite(motorAin2Pin,LOW);
    analogWrite(motorApwmPin,0);
  }
}


void setMotorB(int motorBSpeed) 
{
  if(motorBSpeed!=0){
    if(motorBSpeed>0) {
      digitalWrite(motorBin1Pin,HIGH);
      digitalWrite(motorBin2Pin,LOW);
    } else {
      digitalWrite(motorBin1Pin,LOW);
      digitalWrite(motorBin2Pin,HIGH);
    }
    analogWrite(motorBpwmPin,abs(motorBSpeed));
  } else {
    digitalWrite(motorBin1Pin,LOW);
    digitalWrite(motorBin2Pin,LOW);
    analogWrite(motorBpwmPin,0);
  }
}

void initMotorA() {
  pinMode(motorAin1Pin,OUTPUT);
  pinMode(motorAin2Pin,OUTPUT);
  pinMode(motorApwmPin,OUTPUT);
  setMotorB(0);
}

void initMotorB() {
  pinMode(motorBin1Pin,OUTPUT);
  pinMode(motorBin2Pin,OUTPUT);
  pinMode(motorBpwmPin,OUTPUT);
  setMotorA(0);
}



char * REQUEST_DRIVE  = "c";
char * REQUEST_PING = "ping";
char * REPLY_PING = "pong";








void setup() {
  Serial.begin(115200);
  
  initMotorA();
  initMotorB();
  // Attach the callback function to the Messenger
  message.attach(messageReady);
}


void emergencyStop() {
  setMotorA(0);
  setMotorB(0);
  steeringServo.detach();
}


unsigned long lastMessageTime = millis();

void loop() {
  // The following line is the most effective way of using Serial and Messenger's callback
  while ( Serial.available() )  message.process(Serial.read ());

  unsigned long timeSinceLastMessage = millis() - lastMessageTime;
  if( timeSinceLastMessage > 3000 ) {
    emergencyStop();
  }

}


// Create the callback function
void messageReady() {
  lastMessageTime = millis();
  int requestNr   = message.readInt();
     

  // first, return the requestNr
  Serial.print(requestNr);

  if( message.checkString(REQUEST_DRIVE) ) {
    int throttle = message.readInt();
    int steeringServoPos = message.readInt();
    
    if(!steeringServo.attached()) {
      steeringServo.attach(steeringServoPin);
    }
    steeringServo.write(map(steeringServoPos,0,180,10,180));
    setMotorA(throttle);
    setMotorB(throttle);
  }
  
  else if(message.checkString(REQUEST_PING)) {
    Serial.print(" ");
    Serial.print(REPLY_PING);
  }

  

  // finally, send a message-end marker
  Serial.write("\r");
  
  
  // flush out any superfluous tokens
  while(message.available()) {
    message.readInt();
  }
  
}


